package com.asa.control_theory.utils;

import com.asa.control_theory.utils.bean.ZhuangTai_Matrix;
import com.asa.hutils.MathHUtils;
import com.asa.hutils.Ni;

/**
 * 传递函数到矩阵的标准型的转换 
 * 	能控标准1型
 * 	能观标准2型
 * 	对角线标准形
 * 	Jordan 标准形
 * 
 */
public class GongShi {
	

	/**
	 * 	能控标准1型
	 * @param a	传递函数分母部分
	 * @param b 传递函数分子部分
	 * @return
	 */
	public static ZhuangTai_Matrix nengkong1(double a[],double b[]) {
		double A[][] = MathHUtils.get0(a.length-1);
		double C[][] = new double[b.length-1][1];
		double B[][] = new double[1][b.length-1];
		double D[][] = new double[1][1];

		for (int i = 0; i < A.length-1; i++) {
			A[i][i+1] = 1;
		}
		for (int i = 1; i < a.length; i++) {
			A[A.length-1][i-1] = -a[a.length-i];
		}
		
		for (int i = 0; i < C.length; i++) {
			C[i][0] = b[b.length-1-i] - a[b.length-1-i]*b[0];
		}
		
		
		B[0][b.length-2] = 1;
		
		D[0][0] = b[0];
		MathHUtils.print(B);

		
		
		ZhuangTai_Matrix result = new ZhuangTai_Matrix();
		result.setA(A);
		result.setB(MathHUtils.zhuanzhi(B));
		result.setC(MathHUtils.zhuanzhi(C));
		result.setD(D);
		return result;
	}
	
	
	
	/**
	 * 	能观标准2型
	 * @param a	传递函数分母部分
	 * @param b 传递函数分子部分
	 * @return
	 */
	public static ZhuangTai_Matrix nengguan2(double a[],double b[]) {
		double A[][] = MathHUtils.get0(a.length-1);
		double C[][] = new double[b.length-1][1];
		double B[][] = new double[1][b.length-1];
		double D[][] = new double[1][1];

		for (int i = 1; i < A.length; i++) {
			A[i][i-1] = 1;
			System.out.println(i+"   "+(i-1));
		}
		for (int i = 1; i < a.length; i++) {
			A[i-1][A.length-1] = -a[a.length-i];
		}
		
		
		
		C[C.length-1][0] = 1;
		
		for (int i = 0; i < B[0].length; i++) {
			B[0][i] = b[b.length-1-i] - a[b.length-1-i]*b[0];
		}
		
		
		D[0][0] = b[0];
//		MathHUtils.print(C);

		ZhuangTai_Matrix result = new ZhuangTai_Matrix();
		result.setA(A);
		result.setB(MathHUtils.zhuanzhi(B));
		result.setC(MathHUtils.zhuanzhi(C));
		result.setD(D);
		return result;
	}
	
	

	/**
	 * 	对角线标准形
	 * 这种东西要对传递函数求特征值，就是要把分母给分解成多项式相乘，然后再把分子分解，很是麻烦
	 * @param p	
	 * @param c
	 * @param b0
	 * @return
	 */
	public static ZhuangTai_Matrix duijiaoxian(double p[],double c[],double b0) {
		
		double A[][] = MathHUtils.get0(p.length);
		double C[][] = new double[p.length][1];
		double B[][] = new double[1][p.length];
		double D[][] = new double[1][1];

		
		for (int i = 0; i < A.length; i++) {
			A[i][i] = p[i];
		}
		
		
		for (int i = 0; i < C.length; i++) {
			C[i][0] = c[i];
		}
		
		
		for (int i = 0; i < B[0].length; i++) {
			B[0][i] = 1;
		}
		
		D[0][0] = b0;
		
		MathHUtils.print(D);
		
		ZhuangTai_Matrix result = new ZhuangTai_Matrix();
		result.setA(A);
		result.setB(MathHUtils.zhuanzhi(B));
		result.setC(MathHUtils.zhuanzhi(C));
		result.setD(D);
		return result;
	}

	
	/**
	 * Jordan 标准形	这个还没测过的
	 * @param p
	 * @param c
	 * @param b0
	 * @param chonggen_index 重根的位置
	 * @return
	 */
	public static ZhuangTai_Matrix jardan(double p[],double c[],double b0,int[] chonggen_index) {
		
		double A[][] = MathHUtils.get0(p.length);
		double C[][] = new double[p.length][1];
		double B[][] = new double[1][p.length];
		double D[][] = new double[1][1];

		
		for (int i = 0; i < A.length; i++) {
			A[i][i] = p[i];
		}
		
		for (int i = 0; i < chonggen_index.length; i++) {
			A[chonggen_index[i]][chonggen_index[i]+1] = 1;
		}
		
		
		
		
		
		for (int i = 0; i < C.length; i++) {
			C[i][0] = c[i];
		}
		
		
		for (int i = 0; i < B.length; i++) {
			B[0][i] = 1;
		}
		for (int i = 0; i < chonggen_index.length; i++) {
			B[0][chonggen_index[i]] = 0;
		}
		
		
		
		
		D[0][0] = b0;
		
		
		
		ZhuangTai_Matrix result = new ZhuangTai_Matrix();
		result.setA(A);
		result.setB(MathHUtils.zhuanzhi(B));
		result.setC(MathHUtils.zhuanzhi(C));
		result.setD(D);
		return result;
	}
	
	
	
	public static void print(ZhuangTai_Matrix zt_m) {
		
		double[][] a = zt_m.getA();
		double[][] b = zt_m.getB();
		double[][] c = zt_m.getC();
		double[][] d = zt_m.getD();
		
		
		System.out.println("输出矩阵A:");
		MathHUtils.print(a);
		
		System.out.println("输出矩阵B:");
		MathHUtils.print(b);

		System.out.println("输出矩阵C:");
		MathHUtils.print(c);

		System.out.println("输出矩阵D:");
		MathHUtils.print(d);

		
		
		
	}
	
	
	
	
	
	
	
	public static void main(String[] args) throws Exception {
		
		
		
		double[]  b= {0,1,3};
		double[] a = {1,3,2};
		nengguan2(a, b);
		
		
		double[][] asa = {{0,-2},{1,-3}};
		double[] juzhengtezhengzhi = MathHUtils.juzhengtezhengzhi(asa);
		System.out.println("特征值：");
		for (int i = 0; i < juzhengtezhengzhi.length; i++) {
			System.out.println(juzhengtezhengzhi[i]);
		}
		System.out.println("==============");
		double b0 = 0;
		double[] p = {-2,-1};
		double[] c = {2,-1};
		ZhuangTai_Matrix duijiaoxian = duijiaoxian(p, c, b0);
		print(duijiaoxian);
//		double[][] juzhengni = MathHUtils.juzhengni(asa);
//		Ni.getNi(a)
		
		
		
		
		
	}
	
	
	
	
	
	
	
	
	
	
	
	
	
	
}
